/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package dca.sistemascontrole.malhadecontrole;

import dca.sistemascontrole.util.ControleConstantes;
import dca.sistemascontrole.view.estimacaoestado.FrameEstadoEstimado;

/**
 *
 * @author Rafael
 */
public class Observador {

    private double ganho1;
    private double ganho2;
    private final double nivel_estimado[];

    public Observador(double ganho1, double ganho2) {
        this.ganho1 = ganho1;
        this.ganho2 = ganho2;
        this.nivel_estimado = new double[2];

        nivel_estimado[0] = 0;
        nivel_estimado[1] = 0;
    }

    public void estimar(long t, double nivelTanqueSuperior, double nivelTanqueInfeior, double usat) {
        double L[] = {ganho1, ganho2};
        double nivel_real[] = {nivelTanqueSuperior, nivelTanqueInfeior};

        //double erro_nivel[] = new double[2];
        // Inverti
        //erro_nivel[0] = nivel_estimado[0] - nivel_real[0];
        //erro_nivel[1] = nivel_estimado[1] - nivel_real[1];
        double G[][] = new double[2][2];
        G[0][0] = 0.9934570;
        G[0][1] = 0;
        G[1][0] = 0.0065215;
        G[1][1] = 0.9934570;

        double H[] = new double[2];
        H[0] = 0.02954299;
        H[1] = 0.00009679;

        double y_estimado[] = new double[2];
        y_estimado[0] = 0;
        y_estimado[1] = nivel_estimado[1];

        double LY[] = new double[2];
        LY[0] = L[0] * (nivel_real[1] - y_estimado[1]);
        LY[1] = L[1] * (nivel_real[1] - y_estimado[1]);

        double GL[] = new double[2];
        GL[0] = G[0][0] * L[0] + G[0][1] * L[1];
        GL[1] = G[1][0] * L[0] + G[1][1] * L[1];

        double Hu[] = new double[2];
        Hu[0] = H[0] * usat;
        Hu[1] = H[1] * usat;

//        nivel_estimado[0] = GL[0] + LY[0] + Hu[0];
//        nivel_estimado[1] = GL[1] + LY[1] + Hu[1];
        double estimado1 = G[0][0]*nivel_estimado[0] + G[0][1] * nivel_estimado[1] + L[0] * (nivelTanqueInfeior - nivel_estimado[1]) + H[0]*usat;
        double estimado2 = G[1][0]*nivel_estimado[0] + G[1][1] * nivel_estimado[1] + L[1] * (nivelTanqueInfeior - nivel_estimado[1]) + H[1]*usat;
        
        nivel_estimado[0] = estimado1;
        nivel_estimado[1] = estimado2;
        System.out.println("nivel estimado 1: " + nivel_estimado[0] + " nivel estimado 2: " + nivel_estimado[1]);

        FrameEstadoEstimado frameEstadoEstimado = FrameEstadoEstimado.getINSTANCE();
        frameEstadoEstimado.addValores(nivelTanqueSuperior, nivel_estimado[0], nivelTanqueInfeior, nivel_estimado[1], t);
    }

    public void setGanho1(float ganho1) {
        this.ganho1 = ganho1;
    }

    public void setGanho2(float ganho2) {
        this.ganho2 = ganho2;
    }

    public double getGanho1() {
        return ganho1;
    }

    public double getGanho2() {
        return ganho2;
    }

}
